/* Copyright (C) 2012  	Mathieu Marleix <mathieu.marleix@aerian.fr>
 *						Doris Vaton <doris.vaton@gmail.com>
 * 
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package com.shaipicku.mjpegcams.armControl;

import android.widget.Toast;

import com.shaipicku.mjpegcams.MjpegCamActivity;
import com.shaipicku.mjpegcams.R;

/**
 * Contains all the commands for moving the arm.
 * @author Mathieu Marleix <mathieu.marleix@aerian.fr>
 * @author Doris Vaton <doris.vaton@gmail.com>
 *
 */
public class ArmControl {

	public static final String TAG = "TEST";

	//Code of the different motor
	public static final String CODE_MOTOR_1 = "s%02x", CODE_MOTOR_2 = "t%02x", CODE_MOTOR_3 = "u%02x", CODE_MOTOR_4 = "v%02x", CODE_MOTOR_5 = "w%02x";
	long currentTimeMillis = System.currentTimeMillis();

	private int NumPile = 0;
	public final static int MIDDLE = 1, RIGHT = 2, LEFT = 0;
	//Information of the positions of the motors and figures
	static int PosMot1, PosMot2, PosMot3, PosMot4, PosMot5;
	int PosFig;

	private MjpegCamActivity mjpegActivity;
	public final static int FIRST_MOTOR = 1, SECOND_MOTOR = 2, THIRD_MOTOR = 3, FOURTH_MOTOR = 4, FIFTH_MOTOR = 5;

	public ArmControl(MjpegCamActivity mjpegActivity) {
		// Set up the custom title
		setMjpegActivity(mjpegActivity);
	}
	/**
	 * Send instructions to the PIC in order to pick the right cube
	 * @param integer NumCube
	 * @param boolean error
	 * @return boolean error
	 */
	public boolean camCube(int NumCube, boolean error) {
		switch(NumCube)
		{
		case ArmControl.LEFT:
			error = mvtOneMotor(1, getPosMot1(), 195, error);
			error = mvtTwoMotors(4,5, getPosMot4(), getPosMot5(),100, 150, error);
			error = mvtTwoMotors(2,3, getPosMot2(), getPosMot3(), 220, 175, error);
			error = mvtTwoMotors(4,5, getPosMot4(), getPosMot5(), 125, 200, error);
			error = mvtOneMotor(3, getPosMot3(), 10, error);
			error = mvtOneMotor(2, getPosMot2(), 180, error);
			break;

		case ArmControl.MIDDLE:
			error = mvtOneMotor(1, getPosMot1(), 128, error);
			error = mvtTwoMotors(4,5, getPosMot4(), getPosMot5(),100, 150, error);
			error = mvtTwoMotors(2,3, getPosMot2(), getPosMot3(), 220, 200, error);
			error = mvtTwoMotors(4,5, getPosMot4(), getPosMot5(), 125, 210, error);
			error = mvtOneMotor(3, getPosMot3(), 10, error);
			error = mvtOneMotor(2, getPosMot2(), 180, error);
			break;

		case ArmControl.RIGHT:
			error = mvtOneMotor(1, getPosMot1(), 70, error);
			error = mvtTwoMotors(4,5, getPosMot4(), getPosMot5(),110, 150, error);
			error = mvtTwoMotors(2,3, getPosMot2(), getPosMot3(), 220, 200, error);
			error = mvtTwoMotors(4,5, getPosMot4(), getPosMot5(), 125, 210, error);
			error = mvtOneMotor(3, getPosMot3(), 10, error);
			error = mvtOneMotor(2, getPosMot2(), 180, error);
			break;
		}
		return error;
	}
	/**
	 * Send instructions to the PIC in order to release the cube 
	 * @param integer NumFig The selected figure
	 * @param boolean error
	 * @return boolean error
	 */
	public boolean camFig(int NumFig,boolean error) {
		switch(NumFig)	{
		case ArmControl.LEFT:
			error = mvtOneMotor(1, getPosMot1(), 200,error);
			error = mvtTwoMotors(2,3, getPosMot2(), getPosMot3(), 50, 30,error);
			error = mvtTwoMotors(4,5, getPosMot4(), getPosMot5(),120, 175,error);	
			error = mvtOneMotor(4, getPosMot4(), 110, error);
			error = mvtOneMotor(3, getPosMot3(), 250, error);
			break;

		case ArmControl.MIDDLE:
			error = mvtOneMotor(1, getPosMot1(), 140,error);
			error = mvtTwoMotors(2,3, getPosMot2(), getPosMot3(), 25, 60,error);
			error = mvtTwoMotors(4,5, getPosMot4(), getPosMot5(),135, 170,error);	
			error = mvtOneMotor(4, getPosMot4(), 125,error);
			error = mvtOneMotor(3, getPosMot3(), 250,error);
			break;

		case ArmControl.RIGHT:
			error = mvtOneMotor(1, getPosMot1(), 80,error);
			error = mvtTwoMotors(2,3, getPosMot2(), getPosMot3(), 40, 60,error);
			error = mvtTwoMotors(4,5, getPosMot4(), getPosMot5(),120, 180,error);	
			error = mvtOneMotor(4, getPosMot4(), 100,error);
			error = mvtOneMotor(3, getPosMot3(), 250,error);
			break;
		}
		return error;
	}
	/**
	 * Give the instructions to the PIC in order to pile up the cubes
	 * depending of the number of cubes already piled
	 * @param integer NumCube The selected cube
	 * @param boolean error
	 * @return boolean error
	 */
	public boolean modePileUp(int NumCube, boolean error){

		switch(NumPile)	 {
		case 0:
			if (NumCube != ArmControl.LEFT)	{
				error = mvtOneMotor(1, getPosMot1(), 250, error);
				error = mvtOneMotor(4, getPosMot4(), 120, error);
				error = mvtOneMotor(2, getPosMot2(), 50, error);    			
				error =  mvtOneMotor(5, getPosMot5(), 110, error);
				break;
			}
			else {
				error = mvtOneMotor(1, getPosMot1(), 250, error);
				error = mvtOneMotor(2, getPosMot2(), 50, error);
				error =  mvtOneMotor(5, getPosMot5(), 110, error);
				break;
			}
		case 1:
			error = mvtOneMotor(1, getPosMot1(), 250, error);
			error = mvtOneMotor(4, getPosMot4(), 120, error);
			error = mvtOneMotor(3, getPosMot3(), 0, error); 
			error = mvtOneMotor(2, getPosMot2(), 80, error);   			
			error =  mvtOneMotor(5, getPosMot5(), 110, error);
			break;
		case 2:
			error = mvtOneMotor(1, getPosMot1(), 250, error);
			error = mvtOneMotor(4, getPosMot4(), 120, error);
			error = mvtOneMotor(3, getPosMot3(), 0, error); 
			error = mvtOneMotor(2, getPosMot2(), 105, error);   			
			error =  mvtOneMotor(5, getPosMot5(), 110, error);
			break;
		}   
		NumPile++;
		if(NumPile > 2)
			NumPile = 0;
		return error;
	}
	/**
	 * Move the arm to pick the cubes then pile up them 
	 * and after put the arm in an initial position
	 * @param integer NumCube The selected cube to pile up.
	 */
	public void pileUp(int NumCube) {    		
		boolean error = false;
		error = camCube(NumCube, error);
		error = modePileUp(NumCube, error);
		error = initPileUp(error);
	}
	/**
	 * Move the arm in order to catch the cube and release it at the right place 
	 * and put the arm back in an initial position
	 * @param integer NumCube The selected cube to move in the figure
	 * @param integerNumFig The figure on which the cube has to go
	 */
	public void armMove(int NumCube, int NumFig) {
		boolean error = false;
		error = camCube(NumCube, error);
		error = camFig(NumFig, error);
		error = initBis(error);
	}
	/**
	 *  Initialize the position of the arm after Bluetooth's connection to the device
	 */
	public void initializeMotors() {
		boolean error = false;
		setPosMot1(128);
		setPosMot2(128);
		setPosMot3(128);
		setPosMot4(128);
		setPosMot5(128);    		
		error = mvtOneMotor(ArmControl.FIRST_MOTOR, getPosMot1(), 250, error);
		error = mvtTwoMotors(ArmControl.SECOND_MOTOR,ArmControl.THIRD_MOTOR, getPosMot2(), getPosMot3(), 180, 20, error);
		error = mvtTwoMotors(ArmControl.FOURTH_MOTOR,ArmControl.FIFTH_MOTOR, getPosMot4(), getPosMot5(),150, 250, error);
	}
	/**
	 * Reposition of the arm after every movement of the arm for all modes excepted for the pile up mode  
	 * @param boolean error
	 * @return boolean error
	 */
	public boolean initBis(boolean error)	{
		error = mvtOneMotor(1, getPosMot1(), 10, error);
		error = mvtTwoMotors(2,3, getPosMot2(), getPosMot3(), 70, 200, error);
		return mvtTwoMotors(4,5, getPosMot4(), getPosMot5(),150, 250, error);
	}
	/**
	 * Initialize the position of the arm in the PileUp mode.
	 * @param boolean error
	 * @return boolean error
	 */
	public boolean initPileUp(boolean error)	{
		error = mvtOneMotor(1, getPosMot1(), 250, error);
		error = mvtTwoMotors(2,3, getPosMot2(), getPosMot3(), 180, 20, error);
		return mvtTwoMotors(4,5, getPosMot4(), getPosMot5(),150, 250, error);
	}
	/**
	 * This function sends the value of the position of a motor via Bluetooth.
	 * The format of the message depend of which output of the PIC is linked to the motor
	 * @param integer MotNum Number of the motor to be moved
	 * @param integer PosVal The value of the position to give to the motor
	 */
	public int cmd(int MotNum, int PosVal) {
		switch(MotNum){
		case FIRST_MOTOR:
			setPosMot1(PosVal);
			return getMjpegActivity().sendMessage(String.format(CODE_MOTOR_1, PosVal));
		case SECOND_MOTOR:
			setPosMot2(PosVal);
			return getMjpegActivity().sendMessage(String.format(CODE_MOTOR_2, PosVal));
		case THIRD_MOTOR:
			setPosMot3(PosVal);
			return getMjpegActivity().sendMessage(String.format(CODE_MOTOR_3, PosVal)) ;
		case FOURTH_MOTOR:
			setPosMot4(PosVal);
			return getMjpegActivity().sendMessage(String.format(CODE_MOTOR_4, PosVal)); 
		case FIFTH_MOTOR:
			setPosMot5(PosVal);
			return getMjpegActivity().sendMessage(String.format(CODE_MOTOR_5, PosVal));
		}
		return -3;
	}

	/**
	 * This function moves a motor from a former position to an new one
	 * 
	 * @param integer MotNum Number of the motor to be moved 
	 * @param integer OldPos The old position of the motor
	 * @param integer NewPos The new position of the motor
	 */
	public boolean mvtOneMotor(int MotNum, int OldPos, int NewPos, boolean errorB) {
		int error = -1;
		if (OldPos < NewPos)
			for (int i = OldPos; i <= NewPos && !errorB; i++){
				error = cmd(MotNum,i);
				displayError(error);
				if(error != 0){
					displayError(error);
					errorB = true;
				}
			}
		else 
			for(int i =  OldPos; i >= NewPos && !errorB; i--){
				error = cmd(MotNum,i);
				if(error != 0){
					displayError(error);
					errorB = true;
				}
			}
		return errorB;
	}
	/**
	 * Display errors return by moving one motor.
	 * @param boolean error The error returned
	 */
	private void displayError(int error){
		if(error == -1)
			mjpegActivity.runOnUiThread(new Thread(new Runnable() {

				@Override
				public void run() {
					// TODO Auto-generated method stub
					Toast.makeText(getMjpegActivity(), R.string.not_connected, Toast.LENGTH_SHORT).show();
				}
			}));
		else if(error == -2)
			mjpegActivity.runOnUiThread(new Thread(new Runnable() {

				@Override
				public void run() {
					// TODO Auto-generated method stub
					Toast.makeText(getMjpegActivity(), R.string.no_data, Toast.LENGTH_SHORT).show();
				}
			}));
		else if(error == -3)
			;
	}
	/**
	 * This function removes the gap between two moving motors simultaneously 
	 * triggering the engine ahead or behind so they will have the same number 
	 * of travel performed during the method call "mvtTwoMotors"
	 * 
	 * @param integer MotNumA Number of the first motor to be moved
	 * @param integer MotNumB Number of the second motor to be moved
	 * @param integer OldPosA The old position of the first motor
	 * @param integer OldPosB The old position of the second motor
	 * @param integer NewPosA The new position of the first motor
	 * @param integer NewPosB The new position of the second motor
	 * @return	An array of 2 integers: 
	 *               The first for the new initial position of the first motor 
	 *               And the second for the new initial position of the second motor. 
	 * 
	 */
	public int[] diffMot(int MotNumA, int MotNumB, int OldPosA, int OldPosB, int NewPosA, int NewPosB, boolean error){
		int newPosInit[] = {OldPosA, OldPosB};
		boolean ADecBDec = (OldPosA > NewPosA) && (OldPosB > NewPosB); //Position's Motor A Decreasing and Position's Motor B Decreasing
		boolean ADecBInc = (OldPosA > NewPosA) && (OldPosB < NewPosB); //Position's Motor A Decreasing and Position's Motor B Increasing
		boolean AIncBDec = (OldPosA < NewPosA) && (OldPosB > NewPosB); //Position's Motor A Increasing and Position's Motor B Decreasing

		//Gap between the new and old position of the motor A 
		int diffA = Math.abs(NewPosA - OldPosA);
		//Gap between the new and old position of  the motorB
		int diffB = Math.abs(NewPosB - OldPosB);
		//Difference between the two gaps : the difference to travel before moving the 2 motor simultaneously
		int diffDiff = Math.abs(diffB-diffA); 
		int newInitA, newInitB;

		if (diffB > diffA && !error)
		{
			//The motor B had the biggest gap : moving the motor B to suppress the difference
			if(ADecBDec || AIncBDec)
				error = mvtOneMotor(MotNumB, OldPosB, newInitB = OldPosB - diffDiff,error);
			else
				error = mvtOneMotor(MotNumB, OldPosB, newInitB = OldPosB + diffDiff,error);
			newPosInit[1] = newInitB;
		}    			
		else if(diffB <= diffA  && !error)
		{
			//The motor A had the biggest gap: moving the motor A to suppress the difference
			if(ADecBDec || ADecBInc)
				error = mvtOneMotor(MotNumA, OldPosA, newInitA = OldPosA - diffDiff,error);
			else
				error = mvtOneMotor(MotNumA, OldPosA, newInitA = OldPosA + diffDiff,error);
			newPosInit[0] = newInitA;
		}	
		return newPosInit;
	}


	/**
	 * This function moves two motors simultaneously.
	 * 
	 * @param integer MotNumA Number of the first motor to be moved
	 * @param integer MotNumB Number of the second motor to be moved
	 * @param integer OldPosA The old position of the first motor
	 * @param integer OldPosB The old position of the second motor
	 * @param integer NewPosA The new position of the first motor
	 * @param integer NewPosB The new position of the second motor
	 */
	public boolean mvtTwoMotors(int MotNumA, int MotNumB, int OldPosA, int OldPosB, int NewPosA, int NewPosB, boolean error) {	

		boolean ADecBDec = (OldPosA > NewPosA) && (OldPosB > NewPosB);
		boolean ADecBInc = (OldPosA > NewPosA) && (OldPosB < NewPosB);
		boolean AIncBDec = (OldPosA < NewPosA) && (OldPosB > NewPosB);
		int errorMotorA = -1, errorMotorB = -1;

		int[] newInitMot = diffMot(MotNumA, MotNumB, OldPosA, OldPosB, NewPosA, NewPosB, error);

		if(ADecBDec)
			for(int i = newInitMot[0], j = newInitMot[1]; (i >= NewPosA) && (j >= NewPosB) && !error; i--, j--)
			{
				errorMotorA = cmd(MotNumA, i);
				errorMotorB = cmd(MotNumB, j);
				if(errorMotorA != 0 || errorMotorB != 0){
					displayError(errorMotorA, errorMotorB);
					error = true;
				}
			}
		else if (ADecBInc)
			for(int i = newInitMot[0], j = newInitMot[1]; (i >= NewPosA) && (j <= NewPosB) && !error; i --, j++)
			{
				errorMotorA = cmd(MotNumA, i);
				errorMotorB = cmd(MotNumB, j);
				if(errorMotorA != 0 || errorMotorB != 0){
					displayError(errorMotorA, errorMotorB);
					error = true;
				}
			}
		else if (AIncBDec)
			for(int i = newInitMot[0], j = newInitMot[1]; (i <= NewPosA) && (j >= NewPosB) && !error; i++, j--)
			{
				errorMotorA = cmd(MotNumA, i);
				errorMotorB = cmd(MotNumB, j);
				if(errorMotorA != 0 || errorMotorB != 0){
					displayError(errorMotorA, errorMotorB);
					error = true;
				}
			}
		else 
			for(int i = newInitMot[0], j = newInitMot[1]; (i <= NewPosA) && (j <= NewPosB) && !error; i++, j++)
			{
				errorMotorA = cmd(MotNumA, i);
				errorMotorB = cmd(MotNumB, j);
				if(errorMotorA != 0 || errorMotorB != 0){
					displayError(errorMotorA, errorMotorB);
					error = true;
				}
			}
		return error;
	}
	/**
	 * Display errors returns by two motors. 
	 * @param integer errorMotorA
	 * @param integer errorMotorB
	 */
	private void displayError(int errorMotorA, int errorMotorB){
		if(errorMotorA == -1 || errorMotorB == -1)
			mjpegActivity.runOnUiThread(new Thread(new Runnable() {

				@Override
				public void run() {
					// TODO Auto-generated method stub
					Toast.makeText(getMjpegActivity(), R.string.not_connected, Toast.LENGTH_SHORT).show();
				}
			}));
		else if(errorMotorA == -2 || errorMotorB == -2)
			mjpegActivity.runOnUiThread(new Thread(new Runnable() {

				@Override
				public void run() {
					// TODO Auto-generated method stub
					Toast.makeText(getMjpegActivity(), R.string.no_data, Toast.LENGTH_SHORT).show();
				}
			}));
		else if(errorMotorA == -3 || errorMotorB == -3)
			;
	}

	// The getters and the setters of the position of the different motors
	public int getPosMot1() {
		return PosMot1;
	}

	public void setPosMot1(int posMot1) {
		PosMot1 = posMot1;
	}

	public int getPosMot2() {
		return PosMot2;
	}

	public void setPosMot2(int posMot2) {
		PosMot2 = posMot2;
	}

	public int getPosMot3() {
		return PosMot3;
	}

	public void setPosMot3(int posMot3) {
		PosMot3 = posMot3;
	}

	public int getPosMot4() {
		return PosMot4;
	}

	public void setPosMot4(int posMot4) {
		PosMot4 = posMot4;
	}

	public int getPosMot5() {
		return PosMot5;
	}

	public void setPosMot5(int posMot5) {
		PosMot5 = posMot5;
	}

	public int getPosFig() {
		return PosFig;
	}

	public void setPosFig(int posFig) {
		PosFig = posFig;
	}

	/**
	 * @return the mjpegActivity
	 */
	public MjpegCamActivity getMjpegActivity() {
		return mjpegActivity;
	}

	/**
	 * @param mjpegActivity the mjpegActivity to set
	 */
	public void setMjpegActivity(MjpegCamActivity mjpegActivity) {
		this.mjpegActivity = mjpegActivity;
	}

}